/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-03-14     86188       the first version
 */
#ifndef APPLICATIONS_HEADER_FILE_PID_H_
#define APPLICATIONS_HEADER_FILE_PID_H_

typedef enum
{
    Rol_Rate = 0,
    Pit_Rate,
    Yaw_Rate,

    Rol_Angle,
    Pit_Angle,
    Yaw_Angle,

    Z_Speed,
    Z_Position,
    X_Speed,
    X_Position,
    Y_Speed,
    Y_Position,

    stableRol,
    stablePit,
    Reserve_9,

    camera_pd,
    Reserve_11,
    Reserve_12,

    Pid_Num
}PID_NUM;
/* tracking differentiator */
typedef struct
{
    float   input;    //input of tracking differentiator
    float   r;        //velocity factor ,the lager the r ，the faster the tracking.however,the noise of differential term is increased too much.
    float   h;        //integration of TD system
    int     N0;       //h0=N0*h（h0:filter factor）
    float   fh;       //
    float   x1;       //state
    float   x2;       //state differential
}TD;
typedef struct
{
    float   eLimit ;          //error limit
    float   iLimit ;          //integration limit
    float   Expect;           //expect
    float   FeedBack;         //feedback
    float   Err;              //error
    float   Prev_Err;         //last error
    float   Integrate;        //integration
    float   Kp;               //contral_parameters P
    float   Ki;               //contral_parameters I
    float   Kd;               //contral_parameters D
    float   OutPut;           //output of controller
    float   OutPut_Limit;     //output limit
    float   Prev_FeedBack;    //last feedback
    float   Dis_Err;          //differential
    float   dt;               //delta-time dt
    uint8_t enableDFilter;    //filter for D term enable flag
    TD      TD_filter;        /* tracking differentiator */
}PID_Struct;

typedef struct
{
    PID_Struct pid[Pid_Num];

    float (*Update)(PID_Struct *pid, float FeedBack);
    float (*angle_Update)(PID_Struct *pid, float FeedBack, float Dis_FeedBack);
    float (*Yaw_angle_Updata)(PID_Struct *pid, float FeedBack);
    float (*error_Updata)(PID_Struct *pid, float error);
    void  (*All_PidInit)(void);
    void  (*pidReset)(PID_NUM num);
    void  (*pidReset2)(void);   //reset contral data during body unlock

}PID_HandleTypedef;
extern PID_HandleTypedef PID_Handle;

#endif /* APPLICATIONS_HEADER_FILE_PID_H_ */
